// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GraphOptimizationTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.Reflection;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The GraphOptimization unit test class
    /// </summary>
    [TestClass]
    public class GraphOptimizationTest
    {
        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on simple square loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseSimpleSquareLoop()
        {
            int numLoops = 1;
            int numPosesPerSide = 1;
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's POReSS$ algorithm on simple square loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoressAloneShouldCloseSimpleSquareLoop()
        {
            int numLoops = 1;
            int numPosesPerSide = 1;
            bool runRotationalPoress = false;
            bool runFullPoress = true;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on simple square loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseSimpleSquareLoop()
        {
            int numLoops = 1;
            int numPosesPerSide = 1;
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on simple subdivided square loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseSimpleSubdividedSquareLoop()
        {
            int numLoops = 1;
            int numPosesPerSide = 30;
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on simple subdivided square loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseSimpleSubdividedSquareLoop()
        {
            int numLoops = 1;
            int numPosesPerSide = 30;
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on multiple square loops
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseMultipleSquareLoops()
        {
            int numLoops = 4;
            int numPosesPerSide = 30;
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on multiple square loops
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseMultipleSquareLoops()
        {
            int numLoops = 4;
            int numPosesPerSide = 30;
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on simple square loop with noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseSimpleSquareLoopWithNoise()
        {
            int numLoops = 1;
            int numPosesPerSide = 1;
            const double Sigma = 0.01;  // amount of noise to add
            const double Epsilon = 0.02;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Add noise (but do not change the pose at [0], because it is treated as the origin)
            Random r = new Random();
            for (int i = 1; i < globalPosesInput.Length; i++)
            {
                globalPosesInput[i] = PerturbPose(globalPosesInput[i], r, Sigma);
            }

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on simple square loop with noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseSimpleSquareLoopWithNoise()
        {
            int numLoops = 1;
            int numPosesPerSide = 1;
            const double Sigma = 0.01;  // amount of noise to add
            const double Epsilon = 0.02;  // amount of error to tolerate in result
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Add noise (but do not change the pose at [0], because it is treated as the origin)
            Random r = new Random();
            for (int i = 1; i < globalPosesInput.Length; i++)
            {
                globalPosesInput[i] = PerturbPose(globalPosesInput[i], r, Sigma);
            }

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on simple subdivided square loop with noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseSimpleSubdividedSquareLoopWithNoise()
        {
            int numLoops = 1;
            int numPosesPerSide = 3;
            const double Sigma = 0.1;  // amount of noise to add
            const double Epsilon = 0.3;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Add noise (but do not change the pose at [0], because it is treated as the origin)
            Random r = new Random();
            for (int i = 1; i < globalPosesInput.Length; i++)
            {
                globalPosesInput[i] = PerturbPose(globalPosesInput[i], r, Sigma);
            }

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on simple subdivided square loop with noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseSimpleSubdividedSquareLoopWithNoise()
        {
            int numLoops = 1;
            int numPosesPerSide = 3;
            const double Sigma = 0.1;  // amount of noise to add
            const double Epsilon = 0.2;  // amount of error to tolerate in result
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Add noise (but do not change the pose at [0], because it is treated as the origin)
            Random r = new Random();
            for (int i = 1; i < globalPosesInput.Length; i++)
            {
                globalPosesInput[i] = PerturbPose(globalPosesInput[i], r, Sigma);
            }

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on multiple square loops with noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseMultipleSquareLoopsWithNoise()
        {
            int numLoops = 4;
            int numPosesPerSide = 30;
            const double Sigma = 0.01;  // amount of noise to add
            const double Epsilon = 0.1;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Add noise (but do not change the pose at [0], because it is treated as the origin)
            Random r = new Random();
            for (int i = 1; i < globalPosesInput.Length; i++)
            {
                globalPosesInput[i] = PerturbPose(globalPosesInput[i], r, Sigma);
            }

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on multiple square loops with noise
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseMultipleSquareLoopsWithNoise()
        {
            int numLoops = 4;
            int numPosesPerSide = 30;
            const double Sigma = 0.01;  // amount of noise to add
            const double Epsilon = 0.1;  // amount of error to tolerate in result
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = (Pose2D[])globalPosesGroundTruth.Clone();

            // Add noise (but do not change the pose at [0], because it is treated as the origin)
            Random r = new Random();
            for (int i = 1; i < globalPosesInput.Length; i++)
            {
                globalPosesInput[i] = PerturbPose(globalPosesInput[i], r, Sigma);
            }

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on simple subdivided square loop with systematic drift
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseSimpleSubdividedSquareLoopWithSystematicDrift()
        {
            int numLoops = 1;
            int numPosesPerSide = 3;
            const double Drift = -0.02;  // amount of systematic drift (radians)
            const double Epsilon = 0.4;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide, Drift);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPoress and Graph-Seidel$ algorithm on simple subdivided square loop with systematic drift
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressGraphSeidelShouldCloseSimpleSubdividedSquareLoopWithSystematicDrift()
        {
            int numLoops = 1;
            int numPosesPerSide = 3;
            const double Drift = -0.02;  // amount of systematic drift (radians)
            const double Epsilon = 0.4;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Generate simple square loop (start and end poses are the same)
            Pose2D[] globalPosesGroundTruth = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide);
            Pose2D[] globalPosesInput = GenerateGlobalPosesForNoiselessSquareMultiLoop(numLoops, numPosesPerSide, Drift);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on $Bldg115$ small loop captured by Eddie odometry
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseBldg115SmallLoopCapturedByEddieOdometry()
        {
            const double Epsilon = 1.5;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Load global poses from file
            string filename = "Test.Robotics.Navigation.TestData.poses_SmallLoop2_unoptimized.txt";
            Pose2D[] globalPosesInput = LoadPosesFromFile(filename, true);

            // Load "ground truth" global poses from file
            string groundTruthFilename = "Test.Robotics.Navigation.TestData.poses_SmallLoop2_optimized_poress_gs_ctf.txt";
            Pose2D[] globalPosesGroundTruth = LoadPosesFromFile(groundTruthFilename, true);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on $Bldg115$ small loop captured by Eddie odometry
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [Ignore]
        public void GraphSeidelAloneShouldCloseBldg115SmallLoopCapturedByEddieOdometry()
        {
            const double Epsilon = 1.5;  // amount of error to tolerate in result
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Load global poses from file
            string filename = "Test.Robotics.Navigation.TestData.poses_SmallLoop2_unoptimized.txt";
            Pose2D[] globalPosesInput = LoadPosesFromFile(filename, true);

            // Load "ground truth" global poses from file
            string groundTruthFilename = "Test.Robotics.Navigation.TestData.poses_SmallLoop2_optimized_poress_gs_ctf.txt";
            Pose2D[] globalPosesGroundTruth = LoadPosesFromFile(groundTruthFilename, true);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on rectangular loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseRectLoop()
        {
            const double Epsilon = 0.1;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Load global poses from file
            string filename = "Test.Robotics.Navigation.TestData.poses_rectLoop_unoptimized.txt";
            Pose2D[] globalPosesInput = LoadPosesFromFile(filename, true);

            // Load "ground truth" global poses from file
            string groundTruthFilename = "Test.Robotics.Navigation.TestData.poses_rectLoop_unoptimized.txt";
            Pose2D[] globalPosesGroundTruth = LoadPosesFromFile(groundTruthFilename, true);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on rectangular loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseRectLoop()
        {
            const double Epsilon = 0.1;  // amount of error to tolerate in result
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Load global poses from file
            string filename = "Test.Robotics.Navigation.TestData.poses_rectLoop_unoptimized.txt";
            Pose2D[] globalPosesInput = LoadPosesFromFile(filename, true);

            // Load "ground truth" global poses from file
            string groundTruthFilename = "Test.Robotics.Navigation.TestData.poses_rectLoop_unoptimized.txt";
            Pose2D[] globalPosesGroundTruth = LoadPosesFromFile(groundTruthFilename, true);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's rPOReSS$ algorithm on big cement loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RotationalPoressAloneShouldCloseBigCementLoop()
        {
            const double Epsilon = 0.1;  // amount of error to tolerate in result
            bool runRotationalPoress = true;
            bool runFullPoress = false;
            bool runGraphSeidel = false;

            // Load global poses from file
            string filename = "Test.Robotics.Navigation.TestData.poses_EV4A-036_CementLoop_FastScan_SDB_GyroCalib.txt";
            Pose2D[] globalPosesInput = LoadPosesFromFile(filename, true);

            // Load "ground truth" global poses from file
            string groundTruthFilename = "Test.Robotics.Navigation.TestData.poses_EV4A-036_CementLoop_FastScan_SDB_GyroCalib.txt";
            Pose2D[] globalPosesGroundTruth = LoadPosesFromFile(groundTruthFilename, true);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Test $Peasley's Graph-Seidel$ algorithm on big cement loop
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphSeidelAloneShouldCloseBigCementLoop()
        {
            const double Epsilon = 0.1;  // amount of error to tolerate in result
            bool runRotationalPoress = false;
            bool runFullPoress = false;
            bool runGraphSeidel = true;

            // Load global poses from file
            string filename = "Test.Robotics.Navigation.TestData.poses_EV4A-036_CementLoop_FastScan_SDB_GyroCalib.txt";
            Pose2D[] globalPosesInput = LoadPosesFromFile(filename, true);

            // Load "ground truth" global poses from file
            string groundTruthFilename = "Test.Robotics.Navigation.TestData.poses_EV4A-036_CementLoop_FastScan_SDB_GyroCalib.txt";
            Pose2D[] globalPosesGroundTruth = LoadPosesFromFile(groundTruthFilename, true);

            // Optimize graph
            Pose2D[] globalPosesOutput = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(globalPosesInput, runRotationalPoress, runFullPoress, runGraphSeidel);

            // Check result
            Assert.IsTrue(globalPosesOutput.Length == globalPosesGroundTruth.Length);
            for (int i = 0; i < globalPosesOutput.Length; i++)
            {
                Pose2D groundtruthPose = globalPosesGroundTruth[i];
                Pose2D outputPose = globalPosesOutput[i];
                Assert.IsTrue(outputPose.EqualsWithinEpsilon(groundtruthPose, Epsilon));
            }
        }

        /// <summary>
        /// Load poses from file.
        /// </summary>
        /// <param name="filename">the filename</param>
        /// <param name="fileIsEmbeddedResource">whether file is an embedded resource (versus a standard file on disk)</param>
        /// <returns>the poses</returns>
        internal static Pose2D[] LoadPosesFromFile(string filename, bool fileIsEmbeddedResource = false)
        {
            System.IO.FileStream fs;
            System.IO.StreamReader reader;
            if (fileIsEmbeddedResource)
            {
                Assembly a = Assembly.GetExecutingAssembly();
                System.IO.Stream s = a.GetManifestResourceStream(filename);
                reader = new System.IO.StreamReader(Assembly.GetExecutingAssembly().GetManifestResourceStream(filename));
            }
            else
            {
                fs = new System.IO.FileStream(filename, System.IO.FileMode.Open, System.IO.FileAccess.Read);
                reader = new System.IO.StreamReader(fs);
            }

            List<Pose2D> poses = new List<Pose2D>();
            while (true)
            {
                string line = reader.ReadLine();
                if (line == null)
                {
                    break;
                }

                string[] items = line.Split(' ');
                if (items.Length != 3)
                {
                    break;
                }

                double x = double.Parse(items[0]);
                double y = double.Parse(items[1]);
                double theta = double.Parse(items[2]);
                poses.Add(new Pose2D(x, y, theta));
            }

            reader.Close();

            Pose2D[] posesToReturn = new Pose2D[poses.Count];
            for (int i = 0; i < poses.Count; i++)
            {
                posesToReturn[i] = poses[i];
            }

            return posesToReturn;
        }

        /// <summary>
        /// Generates global 2D poses for a robot traversing a square multiple times.
        /// </summary>
        /// <param name="numLoops">number of loops around the square</param>
        /// <param name="numPosesPerSide">number of poses along each side</param>
        /// <param name="rotationalDrift">systematic rotational drift added to each successive pose (in radians)</param>
        /// <returns>global poses</returns>
        internal static Pose2D[] GenerateGlobalPosesForNoiselessSquareMultiLoop(int numLoops, int numPosesPerSide, double rotationalDrift = 0)
        {
            const int NumSidesPerSquare = 4;
            Pose2D[] poses = new Pose2D[NumSidesPerSquare * numLoops * numPosesPerSide + 1];
            double dx = 1;
            double dy = 0;
            double angle = 0;
            double x = 0;
            double y = 0;
            int index = 0;
            for (int i = 0; i < NumSidesPerSquare * numLoops; i++)
            {
                for (int j = 0; j < numPosesPerSide; j++)
                {
                    poses[index++] = new Pose2D(x, y, angle);
                    x += dx;
                    y += dy;
                    angle += rotationalDrift;
                    dx = Math.Cos(angle);
                    dy = Math.Sin(angle);
                }

                angle += Math.PI / 2.0;
                dx = Math.Cos(angle);
                dy = Math.Sin(angle);
            }

            poses[index] = new Pose2D(x, y, angle);

            return poses;
        }

        /// <summary>
        /// Perturbs a pose by a random amount of Gaussian noise.
        /// </summary>
        /// <param name="pose">Input pose</param>
        /// <param name="r">Random number generator</param>
        /// <param name="sigma">Standard deviation of Gaussian</param>
        /// <returns>Perturbed pose</returns>
        internal static Pose2D PerturbPose(Pose2D pose, Random r, double sigma)
        {
            return new Pose2D(pose.X + (2 * r.NextDouble() - 1) * sigma, pose.Y + (2 * r.NextDouble() - 1) * sigma, pose.Heading + (2 * r.NextDouble() - 1) * sigma);
        }
    }
}
